#!/usr/bin/env python3
# coding:utf-8

import cv2
import numpy as np
import math
import rospy
from std_msgs.msg import ColorRGBA
from std_msgs.msg import Int8
def nothing(x):
    pass

def imgCanny(frame):
    img_g = cv2.GaussianBlur(frame, (5, 5), 0)
    g_img = cv2.cvtColor(img_g, cv2.COLOR_BGR2GRAY)
    ret, th1 = cv2.threshold(g_img, 154, 255, cv2.THRESH_BINARY)
    return th1

def imgeCircle(frame):
    lhsv = np.array([35,43,46])
    hhsv = np.array([77,255,255])
    gs_frame = cv2.GaussianBlur(frame, (5, 5), 0)
    grid_HSV = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(grid_HSV, lhsv, hhsv)
    #cv2.imshow("mask", mask)
    return mask

def hsvFind(frame, mask):
    img_HSV = cv2.bitwise_and(frame, frame, mask=mask)
    #cv2.imshow("realmask", img_HSV)
    frame_gray = cv2.cvtColor(img_HSV, cv2.COLOR_BGR2GRAY)
    return frame_gray

def findCircle(frame_gray, frame):
    circles = cv2.HoughCircles(frame_gray, cv2.HOUGH_GRADIENT, 1, 90, param1=100, param2=25, minRadius=75, maxRadius=170)
    n = 0
    num = []
    if circles is not None:
        circles = np.uint16(np.around(circles))
        #print(circles[0])

        for i in circles[0, :]:
            cv2.circle(frame, (i[0], i[1]), i[2], (0, 0, 255), 2)
            cv2.circle(frame, (i[0], i[1]), 2, (0, 0, 255), -1)
            str1 = '(' + str(i[0]) + ',' + str(i[1]) + ')'
            # cv2.putText(frame, str1, (i[0] + 10, i[1] + 10),
            # cv2.FONT_HERSHEY_COMPLEX, 0.6, (0, 0, 0), 1)
            cv2.putText(frame, str1, (i[0] + 10, i[1] + 10),
                        cv2.FONT_HERSHEY_COMPLEX, 0.6, (0, 0, 255), 1)
            #print("({}, {})".format(i[0], i[1]))
            number = [i[0], i[1]]
            n = 1
            return number
    if n != 1:
        return num

def detelGreen(frame, mask):
    if cv2.countNonZero(mask) > 0:
        #img_mask = cv2.bitwise_and(frame, frame, mask=mask)
        #img_mask = img_mask.shape[:2]
        frame[mask == 255] = (255, 255, 255)
        i = 1
    #cv2.imshow("ff", frame)
    return frame
def state_cb(data):
    global flag
    flag=data.data
    #rospy.loginfo(flag)

def findBlackline(Canny, img):
    lines = cv2.HoughLinesP(Canny, 1, np.pi / 180, 100, minLineLength=20, maxLineGap=80)
    tar = []
    i = 0
    num = []
    if lines is not None:
        for line in lines:
            x1 = int(round(line[0][0]))

            y1 = int(round(line[0][1]))

            x2 = int(round(line[0][2]))

            y2 = int(round(line[0][3]))
            tar.append(x1)
            #print(i)
        num = sorted(tar, reverse=True)
        number = []

        for lin in lines:
            if len(num) >= 3:
                if num[0] == int(round(lin[0][0])):
                    x1 = int(round(lin[0][0]))

                    y1 = int(round(lin[0][1]))

                    x2 = int(round(lin[0][2]))

                    y2 = int(round(lin[0][3]))
                    number.append(x1)
                    number.append(y1)
                    number.append(x2)
                    number.append(y2)
                    e = number[1] - number[3]
                    i = 1
                    if abs(e) >= 190:
                        #print(1)

                        cv2.line(img, (number[0], number[1]), (number[2], number[3]), (0, 0, 255), 10)
                        #print(number)
                        return number
                    else:
                        return num


    if i != 1:
        return num

rospy.init_node('vision_down', anonymous=True)
vision_pub = rospy.Publisher('/data',ColorRGBA, queue_size=10)
sub=rospy.Subscriber("/state", Int8, state_cb)
vision_data=ColorRGBA()
flag = 0
center = [320, 240]
cap = cv2.VideoCapture(7)
# cv2.namedWindow('Canny')
#cv2.namedWindow('CannyBlackcircle')
#cv2.createTrackbar('n', 'CannyBlackcircle', 1, 255, nothing)
# cv2.createTrackbar('max', 'Canny', 1, 255, nothing)
# cv2.createTrackbar('min', 'Canny', 1, 255, nothing)
# cv2.setTrackbarPos('min', 'Canny', 22)
# cv2.setTrackbarPos('max', 'Canny', 44)
#cv2.setTrackbarPos('n', 'CannyBlackcircle', 139)
# cv2.namedWindow('dilate')
# cv2.createTrackbar('n', 'dilate', 1, 255, nothing)
dupraw = []
pupraw = []
Blackcirclenumber = []
while not rospy.is_shutdown() and cap.isOpened():
    frame = cap.read()[1]
    imge = frame.copy()
    imgGreencircle = frame.copy()
    imgBlackcircle = frame.copy()
    # mix = cv2.getTrackbarPos('min', 'Canny')
    # max = cv2.getTrackbarPos('max', 'Canny')
    # n = cv2.getTrackbarPos('n', 'dilate')
    img_gs = cv2.GaussianBlur(imge, (5, 5), 0)
    gs_img = cv2.cvtColor(img_gs, cv2.COLOR_BGR2GRAY)
    Canny = cv2.Canny(gs_img, 22, 44)
    #cv2.imshow("Canny", Canny)
    kernel = np.ones((5, 5), np.uint8)
    dilate = cv2.dilate(Canny, kernel, iterations=1)
    number = findBlackline(Canny, imge)
    #cv2.imshow("dilate", dilate)
    r = 0
    g = 0
    m = 0
    if flag == 1:

        #a = cv2.getTrackbarPos('n', 'CannyBlackcircle')
        mask1 = imgeCircle(imgBlackcircle)
        imgBlackcircle = detelGreen(imgBlackcircle, mask1)
        CannyBlackcircle = imgCanny(imgBlackcircle)
        Blackcirclenumber = findCircle(CannyBlackcircle, imge)
        #rospy.loginfo(m)
        #cv2.imshow('CannyBlackcircle',CannyBlackcircle)
        if len(Blackcirclenumber) == 2:
            if len(pupraw) <= 20:
                pupraw.append(1)
            x_error = Blackcirclenumber[0] - center[0]
            y_error = Blackcirclenumber[1] - center[1]
            # if abs(x_error) <= 5 and y_error >= 40 and y_error <= 45:
            #     pass
            m = 3
            r = -x_error
            g = -y_error
            print("landing")
            print(f"({g}, {r})")
        #rospy.loginfo(m)
        else:
            if len(pupraw) <= 10:
                if len(number) == 4:
                    m = 1
                    error = number[0] - center[0]
                    r = -error
                    #print(-error)
                    try:
                        angle = math.atan((number[0] - number[2]) / (number[1] - number[3]))
                        g = angle / 3.1415926 * 180
                        #print(angle)
                    except:
                        angle = 0
                        m = 0
                print("line 2")
                print(f"({g}, {r})")
    else:
        #rospy.loginfo(m)
        mask = imgeCircle(imgGreencircle)
        # cv2.imshow("mask", mask)
        frame_gray = hsvFind(imgGreencircle, mask)
        Cannny = cv2.Canny(mask, 20, 50)
        # cv2.imshow('C', Cannny)
        circlenum = findCircle(Cannny, imge)
        if len(circlenum) == 2:
            if(len(dupraw)<20):
                dupraw.append(1)
            x_error = circlenum[0] - center[0]
            y_error = circlenum[1] - center[1]
            if abs(x_error) <=5 and y_error >= 40 and y_error <= 45:
                pass
            else:
                m = 2
                r = -x_error
                g = -y_error
                print("Green Circle")
                print(f"{g}, {r}")
            #print(m)
        else:
            if len(dupraw) <= 10:
                if len(number) == 4:
                    m = 1
                    error = number[0] - center[0]
                    r = -error
                    #print(-error)
                    try:
                        angle = math.atan((number[0] - number[2])/(number[1] - number[3]))
                        g = angle / 3.1415926 * 180
                        #print(angle)
                    except:
                        angle = 0
                        m = 0
                print("line 1")
                print(f"({g}, {r})")
    vision_data.r = r
    vision_data.g = g
    vision_data.b = m
    vision_data.a = 0
    vision_pub.publish(vision_data)
    #cv2.imshow("imge", imge)
    if cv2.waitKey(1) == ord(' '):
        break
# 释放摄像头
cap.release()
# 销毁窗口
cv2.destroyAllWindows()
